#ifndef MODULARROBOT_H
#define	MODULARROBOT_H

//#define DEBUG_CONTROLLER
//#define DEBUG_TIMING

#include "EVAlgorithm.h"

#include "Vector_3.h"

#include <webots/Robot.hpp>

#include <vector>
#include <fstream>

using namespace webots;

typedef std::vector<double> dvector;
typedef std::vector<dvector> doubledvector;

class ModularRobot
:
public Robot
{
  public:

    static const std::string GPS_NAME;
    static const std::string EMITTER_NAME;
    static const std::string RECEIVER_NAME;

    static const std::string BUFFER_NAME;
    static const std::string RESULTS_DIR;
    
    static const int GPS_LOG_PERIDO;
    static const int SUPERVISOR_CHANNEL;

    enum Module_Types
    {
        M_STATIC = 0x00,
        M_JOINT = 0x01,
        M_ROOT = 0x02
    };

    enum Algorithm_Types
    {
        A_UNDEFINED = 0,
        A_NEAT = 1,
        A_POWER = 2,
        A_CPG = 3,
        A_SPLINENEAT = 4
    };

    ModularRobot();

    virtual ~ModularRobot();

    void run();

  private:
    boost::property_tree::ptree * _parameters;

    std::string _name;

    std::string logDirectory;

    unsigned int _seed;

    std::size_t _r_index;
    std::size_t _r_index_root;
    std::size_t organismSize;

    std::size_t _m_index;
    int _m_type;

    double _time_step;
    double _time_start;
    double _time_offset;
    double _time_end;

    int _ev_type;
    std::size_t _ev_step;
    std::size_t _ev_steps_recovery;
    std::size_t _ev_steps_total;
    unsigned int totalEvaluations;
    unsigned int evaluation;
    unsigned int generation;
    double _ev_angular_velocity;

    transforms::Vector_3 _position_start;
    transforms::Vector_3 _position_end;

    unsigned int numMotors;
    double motorRange;
    
    EVAlgorithm * _algorithm;

    /* Measurement Devices */
    GPS * _gps;

    /* Communication Devices */
    Emitter * _emitter;
    Receiver * _receiver;

    /* Servos */
    Motor ** _motors;
    
    std::ofstream gpsLog;


    boost::property_tree::ptree * _init_parameters(const std::string &);

    GPS * _init_gps(double);
    Emitter * _init_emitter(int);
    Receiver * _init_receiver(double, int);
    Motor ** _init_motors(double);

    std::string _init_directory(const std::string &, const std::string &, const std::string &, const std::string &logdir, unsigned int);
    transforms::Vector_3 _init_shape_size(const boost::property_tree::ptree &);
    std::vector<transforms::Vector_3> initialiseModuleMapping(std::size_t, std::size_t, const std::string &, const boost::property_tree::ptree &);

    void sendAngles(doubledvector anglesOut);
    void sendAngles(size_t index, dvector anglesOut);
    
    doubledvector receiveAngles();
    dvector receiveAngles(size_t index);

    /**
     * Gets the current position from GPS ignoring the Y axis
     * In our Webots simulations the Y axis is the height
     * @return
     */
    transforms::Vector_3 _get_gps();

    /**
     * Get the current servo angle value normalizing it between [-1;1]
     * @return Servo angle value normalized between [-1;1]
     */
    double _get_motor_position(size_t index);

    /**
     * Sets the servo angle normalizing it between [-pi/2;pi/2]
     * @param Servo angle value normalized between [-1;1]
     */
    void _set_motor_position(size_t index, double value);

    /**
     * Queries the algorithm
     * @param Servos angle values at the current time step
     * @return Servos angle values for the next time step
     */
    std::vector<std::vector<double> > _compute_angles(const std::vector<std::vector<double> > &);

    /**
     * Calculates the controller fitness
     * @param
     * @param
     * @return
     */
    std::pair<double, std::string> _compute_fitness(double, const transforms::Vector_3 &);
    
    double getRealFitness(double fitness);
    
    void logGPS();
    
    bool isRoot();
};

#endif	/* MODULARROBOT_H */


